%% 轨迹分析器
classdef TrajectoryAnalyzer

    properties
        x
        y
        theta
        D
        delta
        forwardNumber
    end
    
    methods

        function obj = TrajectoryAnalyzer(x,y,theta,D,delta,forwardNumber)
            obj.forwardNumber = forwardNumber;
            newx = x;
            newy = y;
            newtheta = theta;
            newD = D;
            newdelta = delta;
            % 为了保证前瞻点数，将最后一个点复制多次
            for i = 1:forwardNumber
                newx = [newx,x(end)];
                newy = [newy,y(end)];
                newtheta = [newtheta,theta(end)];
                newD = [newD,D(end)];
                newdelta = [newdelta,delta(end)];
            end
            obj.x = x;
            obj.y = y;
            obj.theta = theta;
            obj.D = D;
            obj.delta = delta;
        end
        
        % 分析当前位置与参考路径
        % 此处的traceId必须由外部传入，而不是使用self.traceId
        function [direction,thetaError,nextId] = AnalyzePath(self,curX,curY,curTheta,traceId)
            % 找到参考路径上距离当前位置最近的点作为目前已经到达的点
            if traceId == 0
                newX = self.x(traceId+1:end);
                newY = self.y(traceId+1:end);
                divX = newX-curX;
                divY = newY-curY;
                pathDistance = sqrt(divX.^2+divY.^2);
                [~,addIndex] = min(pathDistance);
                posId = traceId+addIndex;
            else
                % 仍然考虑之前已经到达的点以避免累计误差，但也将拖慢转角运行速度
                newX = self.x(traceId:end);
                newY = self.y(traceId:end);
                divX = newX-curX;
                divY = newY-curY;
                pathDistance = sqrt(divX.^2+divY.^2);
                [~,addIndex] = min(pathDistance);
                posId = traceId+addIndex-1;
            end

            % 绘制到达点
            % plot(self.x(posId),self.y(posId),'bo');

            % 获取下一步速度方向
            direction = self.D(posId);

            % 同方向查找前瞻点
            errors = [];
            for i = posId+1:length(self.x)
                % 点(x,y)的来源方向与当前方向不同，则不可以作为前瞻点
                if direction * self.D(i-1) < 0
                    break;
                end

                % 若已经找到足够多的前瞻点，则退出
                if length(errors) < self.forwardNumber
                    % 计算当前位置与参考路径上某点的夹角
                    angle = atan2(self.y(i)-curY,self.x(i)-curX);
                    if direction > 0
                        error = Mod2Pi(angle-curTheta);
                    else
                        error = Mod2Pi(angle-(curTheta+pi));
                    end
                    errors = [errors,error]; %#ok<*AGROW>
                else
                    break;
                end
            end

            % 返回下一步的跟踪索引
            nextId = posId;

            % 计算夹角误差的平均值
            thetaError = mean(errors);
        end
    end
end

% 把弧度x控制在[-pi,pi]
function v = Mod2Pi(x)
    v = rem(x,2*pi); % 求整除x/2pi的余数
    if v < -pi
        v = v+2*pi;
    elseif v > pi
        v = v-2*pi;
    end
end